Semi-autonomous Avoidance of Moving Hazards for Passenger Vehicles

This paper presents a method for semi-autonomous hazard avoidance in the presence of unknown moving obstacles and unpredictable driver inputs. This method iteratively predicts the motion and anticipated intersection of the host vehicle with both static and dynamic hazards and excludes projected collision states from a traversable corridor. A model predictive controller iteratively replans a stability-optimal trajectory through the navigable region of the environment while a threat assessor and semi-autonomous control law modulate driver and controller inputs to maintain stability, preserve controllability, and ensure safe hazard avoidance. The efficacy of this approach is demonstrated through both simulated and experimental results using a semi-autonomously controlled Jaguar S-Type. INTRODUCTION Recent traffic safety reports from the National Highway Traffic and Safety Administration show that in 2007 alone, over 41,000 people were killed and another 2.5 million injured in motor vehicle accidents in the United States [1]. The longstanding presence of passive safety systems in motor vehicles, combined with the ever-increasing influence of active systems, has contributed to a decline in these numbers from previous years. Still, the opportunity for improved collision avoidance technologies remains significant. Recent developments in onboard sensing, lane detection, obstacle recognition, and drive-by-wire capabilities have facilitated active safety systems that autonomously or semiautonomously assist in the driving task [2]. In addition to avoiding static hazards, these systems must account for the motion of dynamic roadway hazards in the presence of uncertain sensor data and unpredictable hazard motion. This consideration, while challenging in its own right, is compounded in semiautonomous vehicle systems, in which a human operator maintains at least partial control of the host vehicle. In these scenarios, the trajectory planner and semi-autonomous controller must allow for (and reject if necessary) unanticipated vehicle state and input disturbances caused by the human operator. In the traditional approach to autonomous vehicle navigation, a path planner and controller are arranged as a tiered subsystem, with a planner designing a collision-free path and the controller seeking to track that path while rejecting disturbances. Common path planning approaches include rapidly-exploring random trees [3], graph search methods [4], potential fields analysis [5], velocity obstacles [6], and neural optimization techniques [7]. Control laws commonly employed in these systems include PID schemes [8], linear-quadratic regulators [9], and nonlinear fuzzy controllers [10]. With a human driver in the loop, tiered subsystem architectures that rely on a pre-planned path may be overly restrictive at best and inaccurate at worst. By seeking to limit the vehicle trajectory to a specific path, these approaches neither allow nor account for deviations from the nominal trajectory caused by human inputs or unanticipated hazard motion. Many existing semi-autonomous systems also seek to perform the hazard avoidance task without explicitly accounting for the effect of driver inputs on the vehicle trajectory [11]. These systems generally estimate the threat posed by static or moving hazards with simple time-based, distance-based, or deceleration-based measures [12-15]. While these metrics provide a useful estimate of threat posed by a given maneuver, they are poorly suited to consider multiple hazards, complex vehicle dynamics, actuator and controller limitations, or complicated environmental geometry with its attendant constraints. In [16], a framework for semi-autonomous control of passenger vehicles is presented. This framework uses Model Predictive Control (MPC) to iteratively plan trajectories through a traversable corridor, assess the threat this trajectory poses to the vehicle, and regulate driver and controller inputs to prevent that threat from exceeding a given threshold. In the context of static hazards, this system’s model-based threat assessment provides an efficient means of: 1) combining static roadway hazards such as lane boundaries and roadway obstacles into realistic spatial constraints and 2) combining these constraints with knowledge of the vehicle dynamics to predict the threat posed by those hazards given the current inputs of a human driver. This paper extends this framework’s hazard avoidance capabilities to account for both static and dynamic hazards and demonstrates (via simulation and experiment) the advantages of this approach over other planning and control techniques designed to avoid static and moving obstacles. Basic framework operation is first presented, followed by a description of the method used to predict the future position of moving roadway hazards, assess the threat each of these hazards poses, and avoid them semi-autonomously. Simulation setup and results are then presented, followed by experimental setup and results, and the paper closes with general conclusions. FRAMEWORK DESCRIPTION The framework described in this paper leverages the predictive and constraint-handling capabilities of MPC to perform trajectory planning, threat assessment, and semi-autonomous hazard avoidance. First, an objective function is established to capture desirable performance characteristics of a safe or “optimal” vehicle path. Boundaries tracing the edges of the drivable road surface are derived from (assumed) forward-looking sensor data and a higher-level corridor planner. These boundaries extrapolate the current state of road hazards (vehicles, pedestrians, etc.) to establish a traversable corridor constraining the vehicle’s projected lateral position. This constraint data, together with a model of the vehicle dynamics is then used to calculate an optimal sequence of inputs and the associated vehicle trajectory. The predicted trajectory is treated as a “best-case” scenario and used to establish the minimum threat posed to the vehicle given its current state and a series of best-case inputs. This threat is then used to calculate the intervention required to prevent departure from the traversable corridor and driver/controller inputs are scaled accordingly. Figure 1 shows a block diagram of this system. FIGURE 1. DIAGRAM OF AN ACTIVE SAFETY SYSTEM Assumptions In this paper it is assumed that road lane data is available and that the instantaneous position, velocity, and acceleration of road hazards have been measured or estimated by on-board sensors or vehicle-to-vehicle communication. Existing systems and previous work in onboard sensing and sensor fusion justify this as a reasonable assumption [17]. Radar, LIDAR, and vision-based lane-recognition systems [3], along with various sensor fusion approaches [18] have been proposed to provide the lane, hazard, and environmental information needed by this framework. Where multiple corridor options exist (such as cases where the roadway branches or the vehicle must avoid an obstacle in the center of the lane), it is assumed that a high-level path planner has selected a single corridor through which the vehicle should travel. Path Planning The best-case (or baseline) path through the constrained corridor is predicted by an MPC controller. Model Predictive Control is a finite-horizon optimal control scheme that uses a model of the plant to predict future vehicle state evolution and optimize a set of inputs such that this prediction satisfies constraints and minimizes a user-defined objective function. At each time step, t, the current plant state is sampled and a costminimizing control sequence spanning from time t to the end of a control horizon of n sampling intervals, t+n∆t, is computed subject to inequality constraints. The first element in this input sequence is implemented at the current time and the process is repeated at subsequent time steps. Three important elements of the controller implemented in this paper – the plant model, objective function, and constraint setup – are described below. Vehicle Dynamic Model. The vehicle model used by the controller consists of the linearized kinematics of a 4-wheeled vehicle along with its lateral (wheel slip) and yaw dynamics. Vehicle states include the position of its center of gravity [x, y], the vehicle yaw angle ψ , yaw rate ψ& , and sideslip angle β, as illustrated in Figure 2. The input to the system is the front steer angle δ. FIGURE 2. VEHICLE MODEL USED IN MPC CONTROLLER Table 1 defines and quantifies this model’s parameters. TABLE 1. VEHICLE MODEL PARAMETERS Symbol Description Value [units] m Total vehicle mass 2050 [kg] Izz Yaw moment of inertia 3344 [kg m^2] xf C.g. distance to front wheels 1.43 [m] xr C.g. distance to rear wheels 1.47 [m] yw Track width 1.44 [m] Cf Front cornering stiffness 1433 [N/deg] Cr Rear cornering stiffness 1433 [N/deg] μ Surface friction coefficient 1 Tire compliance is included in the model by approximating lateral tire force (Fy) as the product of wheel cornering stiffness (C) and wheel sideslip (α or β) as in